#include "ros/ros.h"
#include "std_msgs/Bool.h"
#include "epuckDriver/BoolMsg.h"

#include "unistd.h"

int main(int argc,char **argv){

    ros::init(argc,argv,"LEDTest");
    ros::NodeHandle n;
    ros::Publisher pubFront;
    ros::Publisher pubBody;
    ros::Publisher pubRing;

    pubFront = n.advertise<std_msgs::Bool>("epuckFrontLED1",1);    
    pubBody = n.advertise<std_msgs::Bool>("epuckBodyLED1",1);    
    pubRing = n.advertise<epuckDriver::BoolMsg>("epuckRingLED1",1);    

    std_msgs::Bool front, body;
    epuckDriver::BoolMsg ring;

    ring.data.resize(8);
    
    
    front.data = true;
    body.data = true;
    for(int i=0;i < 8; i++)
        ring.data[i] = true;
   
    pubFront.publish(front); 
    pubBody.publish(body); 
    pubRing.publish(ring); 
   
    sleep(2);
    
    front.data = false;
    body.data = false;
    for(int i=0;i < 8; i++)
        ring.data[i] = false;

    pubFront.publish(front); 
    pubBody.publish(body); 
    pubRing.publish(ring); 

    pubFront.publish(front); 
    sleep(2);
    
    return 0;
}
